home *** CD-ROM | disk | FTP | other *** search
/ Aminet 6 / Aminet 6 - June 1995.iso / Aminet / gfx / 3d / irit50src.lha / irit5 / xtra_lib / nure_svd.c < prev   
Encoding:
C/C++ Source or Header  |  1994-09-11  |  9.1 KB  |  340 lines
  1. /******************************************************************************
  2. * Numerical Recipes' SVD singular value decomposition code.              *
  3. * "Numerical Recipes in C", by William H. Press el al., University of          *
  4. * Cambridge, 1988, ISBN 0521-35465-X.                          *
  5. *   Really ugly piece of code, that works quite well.                  *
  6. ******************************************************************************/
  7.  
  8. #include <math.h>
  9. #include <irit_sm.h>
  10. #include <imalloc.h>
  11.  
  12. #define SVD_TOL 1.0e-5
  13.  
  14. #define SIGN2(a, b)    ((b) >= 0.0 ? fabs(a) : -fabs(a))
  15. #define PYTHAG(a, b)    sqrt(SQR(a) + SQR(b))
  16. #define SVD_VECTOR(n)    ((RealType *) IritMalloc(sizeof(RealType) * (n + 1)))
  17. #define SVD_FREE_VEC(p)    IritFree((VoidPtr) p)
  18.  
  19. static void svdcmp(RealType **a, int m, int n, RealType *w, RealType **v);
  20.  
  21. /*****************************************************************************
  22. * DESCRIPTION:                                                               *
  23. * Singular value decomposition of matrix A into A W V.                 *
  24. *   Given A is m by n, m >= n.                             *
  25. *   Output:                                     *
  26. *    A is m by n modified in place into U,                     *
  27. *    W is a vector of length m,                         *
  28. *    V is a square matrix of size n by n.                     *
  29. *   Due to the Fortran style of this code all indices are from 1 to k, so    *
  30. * an array from 1 to k actually have k + 1 elements in this C translation.   *
  31. *                                                                            *
  32. * PARAMETERS:                                                                *
  33. *   a:    A m by m matrix.                                                   *
  34. *   m, n: Dimensions of A, W and V                         *
  35. *   w:    A vector of length m                             *
  36. *   v:    A matrix of size n by n.                         *
  37. *                                                                            *
  38. * RETURN VALUE:                                                              *
  39. *   void                                                                     *
  40. *****************************************************************************/
  41. static void svdcmp(RealType **a, int m, int n, RealType *w, RealType **v)
  42. {
  43.     int flag, i, its, j, jj, k, l, nm;
  44.     RealType c, f, h, s, x, y, z, *rv1;
  45.     RealType
  46.     anorm = 0.0,
  47.     g = 0.0,
  48.     scale = 0.0;
  49.  
  50.     if (m < n)
  51.     IritFatalError("SVDCMP: You must augment A with extra zero rows");
  52.  
  53.     rv1 = SVD_VECTOR(n);
  54.     for (i = 1; i <= n; i++) {
  55.     l = i + 1;
  56.     rv1[i] = scale * g;
  57.     g = s = scale = 0.0;
  58.     if (i <= m) {
  59.         for (k = i; k <= m; k++)
  60.         scale += fabs(a[k][i]);
  61.         if (scale) {
  62.         for (k = i; k <= m; k++) {
  63.             a[k][i] /= scale;
  64.             s += a[k][i] * a[k][i];
  65.         }
  66.         f = a[i][i];
  67.         g = -SIGN2(sqrt(s),f);
  68.         h = f * g - s;
  69.         a[i][i] = f - g;
  70.         if (i != n) {
  71.             for (j = l; j <= n; j++) {
  72.             for (s = 0.0, k = i; k <= m; k++)
  73.                 s += a[k][i]*a[k][j];
  74.             f = s / h;
  75.             for (k = i; k <= m; k++)
  76.                 a[k][j] += f*a[k][i];
  77.             }
  78.         }
  79.         for (k = i; k <= m; k++)
  80.             a[k][i] *= scale;
  81.         }
  82.     }
  83.     w[i] = scale * g;
  84.     g = s = scale = 0.0;
  85.     if (i <= m && i != n) {
  86.         for (k = l; k <= n; k++)
  87.         scale += fabs(a[i][k]);
  88.         if (scale) {
  89.         for (k = l; k <= n; k++) {
  90.             a[i][k] /= scale;
  91.             s += a[i][k]*a[i][k];
  92.         }
  93.         f = a[i][l];
  94.         g = -SIGN2(sqrt(s), f);
  95.         h = f * g - s;
  96.         a[i][l] = f - g;
  97.         for (k = l; k <= n; k++)
  98.             rv1[k]=a[i][k]/h;
  99.         if (i != m) {
  100.             for (j = l;j <= m; j++) {
  101.             for (s = 0.0, k=l; k <= n; k++)
  102.                 s += a[j][k] * a[i][k];
  103.             for (k = l; k <= n; k++)
  104.                 a[j][k] += s * rv1[k];
  105.             }
  106.         }
  107.         for (k=l;k<=n;k++) a[i][k] *= scale;
  108.         }
  109.     }
  110.     s = fabs(w[i])+fabs(rv1[i]);
  111.     anorm = MAX(anorm, s);
  112.     }
  113.     for (i = n; i >= 1; i--) {
  114.     if (i < n) {
  115.         if (g) {
  116.         for (j = l; j <= n; j++)
  117.             v[j][i] = (a[i][j] / a[i][l]) / g;
  118.         for (j = l; j <= n; j++) {
  119.             for (s = 0.0, k = l; k <= n; k++)
  120.             s += a[i][k]*v[k][j];
  121.             for (k = l; k <= n; k++)
  122.             v[k][j] += s * v[k][i];
  123.         }
  124.         }
  125.         for (j = l; j <= n; j++)
  126.         v[i][j]=v[j][i]=0.0;
  127.     }
  128.     v[i][i] = 1.0;
  129.     g = rv1[i];
  130.     l = i;
  131.     }
  132.     for (i = n; i >= 1; i--) {
  133.     l = i+1;
  134.     g = w[i];
  135.     if (i < n)
  136.         for (j = l;j <= n;j++)
  137.         a[i][j]=0.0;
  138.     if (g) {
  139.         g=1.0 / g;
  140.         if (i != n) {
  141.         for (j = l; j <= n; j++) {
  142.             for (s = 0.0, k = l; k <= m; k++)
  143.             s += a[k][i] * a[k][j];
  144.             f = (s / a[i][i]) * g;
  145.             for (k = i; k <= m; k++)
  146.             a[k][j] += f * a[k][i];
  147.         }
  148.         }
  149.         for (j = i; j <= m; j++)
  150.         a[j][i] *= g;
  151.     }
  152.     else {
  153.         for (j = i; j <= m; j++)
  154.         a[j][i] = 0.0;
  155.     }
  156.     ++a[i][i];
  157.     }
  158.     for (k = n; k >= 1; k--) {
  159.     for (its = 1; its <= 30; its++) {
  160.         flag = 1;
  161.         for (l = k; l >= 1; l--) {
  162.         nm = l-1;
  163.         if (fabs(rv1[l]) + anorm  ==  anorm) {
  164.             flag = 0;
  165.             break;
  166.         }
  167.         if (fabs(w[nm]) + anorm == anorm)
  168.             break;
  169.         }
  170.         if (flag) {
  171.         c = 0.0;
  172.         s = 1.0;
  173.         for (i = l; i <= k; i++) {
  174.             f = s * rv1[i];
  175.             if (fabs(f) + anorm != anorm) {
  176.             g = w[i];
  177.             h = PYTHAG(f, g);
  178.             w[i] = h;
  179.             h = 1.0 / h;
  180.             c = g * h;
  181.             s = (-f * h);
  182.             for (j = 1; j <= m; j++) {
  183.                 y = a[j][nm];
  184.                 z = a[j][i];
  185.                 a[j][nm] = y * c + z * s;
  186.                 a[j][i] = z * c - y * s;
  187.             }
  188.             }
  189.         }
  190.         }
  191.         z = w[k];
  192.         if (l == k) {
  193.         if (z < 0.0) {
  194.             w[k] = -z;
  195.             for (j = 1; j <= n; j++)
  196.             v[j][k] = (-v[j][k]);
  197.         }
  198.         break;
  199.         }
  200.         if (its == 30)
  201.         IritFatalError("No convergence in 30 SVDCMP iterations");
  202.         x = w[l];
  203.         nm = k - 1;
  204.         y = w[nm];
  205.         g = rv1[nm];
  206.         h = rv1[k];
  207.         f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
  208.         g = PYTHAG(f, 1.0);
  209.         f = ((x - z) * (x + z) + h * ((y / (f + SIGN2(g, f))) - h)) / x;
  210.         c = s = 1.0;
  211.         for (j = l; j <= nm; j++) {
  212.         i = j + 1;
  213.         g = rv1[i];
  214.         y = w[i];
  215.         h = s * g;
  216.         g = c * g;
  217.         z = PYTHAG(f, h);
  218.         rv1[j] = z;
  219.         c = f / z;
  220.         s = h / z;
  221.         f = x * c + g * s;
  222.         g = g * c - x * s;
  223.         h = y * s;
  224.         y = y * c;
  225.         for (jj = 1; jj <= n; jj++) {
  226.             x = v[jj][j];
  227.             z = v[jj][i];
  228.             v[jj][j] = x * c + z * s;
  229.             v[jj][i] = z * c - x * s;
  230.         }
  231.         z = PYTHAG(f, h);
  232.         w[j] = z;
  233.         if (z) {
  234.             z = 1.0 / z;
  235.             c = f * z;
  236.             s = h * z;
  237.         }
  238.         f = (c * g) + (s * y);
  239.         x = (c * y) - (s * g);
  240.         for (jj = 1; jj <= m; jj++) {
  241.             y = a[jj][j];
  242.             z = a[jj][i];
  243.             a[jj][j] = y * c + z * s;
  244.             a[jj][i] = z * c - y * s;
  245.         }
  246.         }
  247.         rv1[l] = 0.0;
  248.         rv1[k] = f;
  249.         w[k] = x;
  250.     }
  251.     }
  252.     SVD_FREE_VEC(rv1);
  253. }
  254.  
  255. /*****************************************************************************
  256. * DESCRIPTION:                                                               M
  257. * Least square solves A x = b.                             M
  258. *   The vector X is of size Nx, vector b is of size NData and matrix A is    M
  259. * of size Nx by NData.                                 M
  260. *   Uses singular value decomposition.                         M
  261. *   If A != NULL is SVD decomposition is computed, otherwise (A == NULL) a   M
  262. * solution is computed for the given b and is placed in x.             M
  263. *                                                                            *
  264. * PARAMETERS:                                                                M
  265. *   A:         The matrix of size Nx by NData.                               M
  266. *   x:         The vector of sought solution of size Nx.                     M
  267. *   b:         The vector of coefficients of size NData.                     M
  268. *   NData, Nx:  Dimensions of input.                                         M
  269. *                                                                            *
  270. * RETURN VALUE:                                                              M
  271. *   void                                                                     M
  272. *                                                                            *
  273. * KEYWORDS:                                                                  M
  274. *   SvdLeastSqr,  singular value decomposition, linear systems               M
  275. *****************************************************************************/
  276. void SvdLeastSqr(RealType *A, RealType *x, RealType *b, int NData, int Nx)
  277. {
  278.     static int
  279.     AllocNData = 0,
  280.     AllocNx = 0;
  281.     static RealType
  282.     **SVD_U = NULL,
  283.     **SVD_V = NULL,
  284.     *SVD_W = NULL;
  285.     int i, j;
  286.  
  287.     if (A != NULL) {
  288.     if (SVD_U != NULL) {          /* Free old instance of aux. data. */
  289.         for (i = 0; i <= AllocNData; i++)
  290.         SVD_FREE_VEC(SVD_U[i]);
  291.         IritFree((VoidPtr) SVD_U);
  292.         for (i = 0; i <= AllocNx; i++)
  293.         SVD_FREE_VEC(SVD_V[i]);
  294.         IritFree((VoidPtr) SVD_V);
  295.         SVD_FREE_VEC(SVD_W);
  296.     }
  297.  
  298.     SVD_U = (RealType **) IritMalloc((NData + 1) * sizeof(RealType *));
  299.     SVD_V = (RealType **) IritMalloc((Nx + 1) * sizeof(RealType *));
  300.     SVD_W = SVD_VECTOR(NData);
  301.     for (i = 0; i <= NData; i++)
  302.         SVD_U[i] = SVD_VECTOR(Nx);
  303.     for (i = 0; i <= Nx; i++)
  304.         SVD_V[i] = SVD_VECTOR(Nx);
  305.     AllocNData = NData;
  306.     AllocNx = Nx;
  307.  
  308.     for (i = 0; i < NData; i++) {
  309.         for (j = 0; j < Nx; j++) {
  310.         SVD_U[i + 1][j + 1] = A[i * Nx + j];
  311.         }
  312.     }
  313.     svdcmp(SVD_U, NData, Nx, SVD_W, SVD_V);
  314.     }
  315.     else {
  316.     RealType
  317.         *TVec = SVD_VECTOR(Nx);
  318.  
  319.     for (j = 1; j <= Nx; j++) {
  320.         RealType s = 0.0;
  321.  
  322.         if (SVD_W[j]) {
  323.         for (i = 1; i <= NData; i++)
  324.             s += SVD_U[i][j] * b[i - 1];
  325.         s /= SVD_W[j];
  326.         }
  327.         TVec[j] = s;
  328.     }
  329.     for (j = 1; j <= Nx; j++) {
  330.         RealType s = 0.0;
  331.         
  332.         for (i = 1; i <= Nx; i++)
  333.         s += SVD_V[j][i] * TVec[i];
  334.         x[j - 1] = s;
  335.     }
  336.  
  337.     SVD_FREE_VEC(TVec);
  338.     }
  339. }
  340.